#pragma once
#include <memory>
#include <cstddef>
#include <functional>

/**
 * @brief 链表节点
 */
template<typename T>
struct ListNode {
    T data;
    std::shared_ptr<ListNode<T>> next;
    ListNode(const T& d): data(d), next(nullptr) {}
    ListNode(T&& d): data(std::move(d)), next(nullptr) {}
};

/**
 * @brief 模板链表类 (T通常为PoseInfo, 相对于robot_init frame)
 * @details 头节点为当前机器人位置, 后续节点为依次到达的目标点
 */
template<typename T>
class LinkedList {
public:
    LinkedList(): head_(nullptr), size_(0) {}

    // 设置头节点（机器人当前位置）
    template<typename U>
    void set_head(U&& value) {
        if (!head_) {
            head_ = std::make_shared<ListNode<T>>(std::forward<U>(value));
            size_ = 1;
        } else {
            head_->data = std::forward<U>(value);
        }
    }

    // 获取index位置的节点, 从0开始 (默认获取第一个节点)
    std::shared_ptr<ListNode<T>> get_node(int index = 0) {
        if (index < 0 || index >= static_cast<int>(size_)) return nullptr; // 越界
        auto node = head_;
        for (int i = 0; i < index && node->next; ++i) {
            node = node->next;
        }
        return node;
    }

    // 添加节点在index的后面, 从0开始 (默认在最后一个节点添加)
    template<typename U>
    bool add_node(U&& value, int index = -1) {
        if (index == -1) index = size_ - 1;
        auto node = get_node(index);
        if (node == nullptr) return false;
        auto next_node = node->next;
        node->next = std::make_shared<ListNode<T>>(std::forward<U>(value));
        node->next->next = next_node;
        ++size_;
        return true;
    }

    // 删除index位置节点, 从0开始
    bool pop_node(int index = 0) {
        if (index == -1) index = size_ - 1;
        auto node = get_node(index - 1);
        if (index != 0 && node == nullptr) return false;
        if (index == 0) { // 删除头节点
            head_ = head_->next;
        } else { // 删除指定位置的节点
            node->next = node->next ? node->next->next : nullptr;
        }
        --size_;
        return true;
    }

    // 移动index1位置的节点到index2的后面, 从0开始
    bool move_node(int index1, int index2) {
        if (index1 == index2) return true;
        auto node1 = get_node(index1);
        auto node2 = get_node(index2);
        if (node1 == nullptr || node2 == nullptr) return false;

        pop_node(index1);
        node1->next = node2->next;
        node2->next = node1;
        ++size_;
        return true;
    }

    // 获取链表头
    std::shared_ptr<ListNode<T>> head() { return head_; }

    // 获取第二个节点
    std::shared_ptr<ListNode<T>> second_node() {
        if (head_ && head_->next) return head_->next;
        return nullptr;
    }

    // 获取链表总元素个数
    size_t size() const { return size_; }

    // 清空链表, 从start_index开始删除, 包括start_index, 从0开始
    void clear(int start_index = 0) {
        if (start_index == 0) head_ = nullptr;
        else {
            auto node = get_node(start_index - 1);
            node->next = nullptr;
        }
        size_ = start_index;
    }

    // 应用函数到链表中的每个元素
    void apply_to_all(const std::function<void(T&)>& func) {
        auto node = head_;
        while (node) {
            func(node->data);
            node = node->next;
        }
    }

private:
    std::shared_ptr<ListNode<T>> head_;
    size_t size_;
};
